#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "stdlib.h"
#include "math.h"
#include "OLED.h"
//PB6-->TIM4_CH1
uint8_t flas=1;//flas代表是否是第一次执行舵机代码
void PWM_Init_PB6(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	

	GPIO_InitTypeDef GPIO_InitStructure; 				
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	TIM_InternalClockConfig(TIM4);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1449 - 1; 
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure); 
	 	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC1Init(TIM4,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM4,ENABLE);
}

void PWM_SetCompare1_PB6(uint16_t Compare)
{
	TIM_SetCompare1(TIM4, Compare);
}

int Servo_SetAngle_PB6(float Angle, float Speed,int flas)
{
	 if(flas==1){
		 PWM_SetCompare1_PB6(Angle / 180 * 100 + 25);
	 }else{
		 float currentAngle;
		 OLED_Init();
			// 获取当前舵机角度
		 currentAngle = (float)((TIM_GetCapture1(TIM4)-25)*1.8); // 假设有一个函数可以获取当前舵机角度
//		 OLED_ShowNum(1,1,currentAngle,3);
//		 OLED_ShowNum(1,5,TIM_GetCapture1(TIM4),3);
			// 计算舵机需要转动的角度差
	//    float angleDiff = fabs(Angle - currentAngle);//取绝对值

			// 计算舵机转动的步长
			float step = Speed * 10;

			// 根据角度差确定递增还是递减
			int direction = 1;
			if (Angle < currentAngle) {
					direction = -1;
			}

			// 逐步增加或减小舵机角度，控制速度
			while (currentAngle != Angle) {
					currentAngle += step * direction;
					if (direction == 1 && currentAngle > Angle) {
							currentAngle = Angle;
					}
					if (direction == -1 && currentAngle < Angle) {
							currentAngle = Angle;
					}
//					OLED_ShowNum(2,1,currentAngle,3);
					// 设置舵机角度
					PWM_SetCompare1_PB6(currentAngle / 180 * 100 + 25);
					// 添加延迟
					Delay_ms(10);
      }
	 }
	 return 2;
}






//PB7-->TIM4_CH2
void PWM_Init_PB7(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	

	GPIO_InitTypeDef GPIO_InitStructure; 				
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	TIM_InternalClockConfig(TIM4);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1449 - 1; 
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure); 
	 	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC2Init(TIM4,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM4,ENABLE);
}

void PWM_SetCompare2_PB7(uint16_t Compare)
{
	TIM_SetCompare2(TIM4, Compare);
}

int Servo_SetAngle_PB7(float Angle, float Speed,int flas)
{
	if(flas==1){
		PWM_SetCompare2_PB7(Angle / 180 * 100 + 25);
	}else{
		    // 获取当前舵机角度
    float currentAngle =(float)((TIM_GetCapture2(TIM4)-25)*1.8) ; // 假设有一个函数可以获取当前舵机角度

    // 计算舵机需要转动的角度差
//    float angleDiff = fabs(Angle - currentAngle);

    // 计算舵机转动的步长
    float step = Speed * 10;

    // 根据角度差确定递增还是递减
    int direction = 1;
    if (Angle < currentAngle) {
        direction = -1;
    }

    // 逐步增加或减小舵机角度，控制速度
    while (currentAngle != Angle) {
        currentAngle += step * direction;
        if (direction == 1 && currentAngle > Angle) {
            currentAngle = Angle;
        }
        if (direction == -1 && currentAngle < Angle) {
            currentAngle = Angle;
        }

        // 设置舵机角度
        PWM_SetCompare2_PB7(currentAngle / 180 * 100 + 25);

        // 添加延迟
        Delay_ms(10);
    }
	}
	return 2;
}





//PB8-->TIM4_CH3
void PWM_Init_PB8(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	

	GPIO_InitTypeDef GPIO_InitStructure; 				
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	TIM_InternalClockConfig(TIM4);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1449 - 1; 
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure); 
	 	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC3Init(TIM4,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM4,ENABLE);
}

void PWM_SetCompare3_PB8(uint16_t Compare)
{
	TIM_SetCompare3(TIM4, Compare);
}

int Servo_SetAngle_PB8(float Angle, float Speed,int flas)
{
	if(flas==1){
		PWM_SetCompare3_PB8(Angle / 180 * 100 + 25);
	}else{
		    // 获取当前舵机角度
    float currentAngle = (float)((TIM_GetCapture3(TIM4)-25)*1.8) ; // 假设有一个函数可以获取当前舵机角度

    // 计算舵机需要转动的角度差
//    float angleDiff = fabs(Angle - currentAngle);
//    if (angleDiff < 0) {
//        angleDiff = -angleDiff; // 取绝对值
//    }

    // 计算舵机转动的步长
    float step = Speed * 10;

    // 根据角度差确定递增还是递减
    int direction = 1;
    if (Angle < currentAngle) {
        direction = -1;
    }

    // 逐步增加或减小舵机角度，控制速度
    while (currentAngle != Angle) {
        currentAngle += step * direction;
        if (direction == 1 && currentAngle > Angle) {
            currentAngle = Angle; 
        }
        if (direction == -1 && currentAngle < Angle) {
            currentAngle = Angle;
        }

        // 设置舵机角度
        PWM_SetCompare3_PB8(currentAngle / 180 * 100 + 25);

        // 添加延迟
        Delay_ms(10);
    }
	}
	return 2;
}

//PB9-->TIM4_CH4
void PWM_Init_PB9(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	

	GPIO_InitTypeDef GPIO_InitStructure; 				
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	TIM_InternalClockConfig(TIM4);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 1449 - 1; 
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseInitStructure); 
	 	
	TIM_OCInitTypeDef TIM_OCInitStructure;
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;
	TIM_OC4Init(TIM4,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM4,ENABLE);
}

void PWM_SetCompare4_PB9(uint16_t Compare)
{
	TIM_SetCompare4(TIM4, Compare);
}
int Servo_SetAngle_PB9(float Angle, float Speed,int flas)
{
	if(flas==1){
		PWM_SetCompare4_PB9(Angle / 180 * 100 + 25);
	}else{
		    // 获取当前舵机角度
    float currentAngle = (float)((TIM_GetCapture4(TIM4)-25)*1.8) ; // 假设有一个函数可以获取当前舵机角度

    // 计算舵机需要转动的角度差
//    float angleDiff = fabs(Angle - currentAngle);
//    if (angleDiff < 0) {
//        angleDiff = -angleDiff; // 取绝对值
//    }

    // 计算舵机转动的步长
    float step = Speed * 10;

    // 根据角度差确定递增还是递减
    int direction = 1;
    if (Angle < currentAngle) {
        direction = -1;
    }

    // 逐步增加或减小舵机角度，控制速度
    while (currentAngle != Angle) {
        currentAngle += step * direction;
        if (direction == 1 && currentAngle > Angle) {
            currentAngle = Angle;
        }
        if (direction == -1 && currentAngle < Angle) {
            currentAngle = Angle;
        }

        // 设置舵机角度
        PWM_SetCompare4_PB9(currentAngle / 180 * 100 + 25);

        // 添加延迟
        Delay_ms(10);
    }
	}
	return 2;
}






//void duoPWM_Init(void)
//{
//	PWM_Init_PB6();
//	PWM_Init_PB7();
//	PWM_Init_PB8();
//	PWM_Init_PB9();
//}

void duoji_Init(void)
{
	PWM_Init_PB6();
	PWM_Init_PB7();
	PWM_Init_PB8();
	PWM_Init_PB9();
	Delay_ms(1000);
	Servo_SetAngle_PB6(90,0,flas);
	Delay_ms(1000);
	flas=Servo_SetAngle_PB7(135,0,flas);
	Delay_ms(1000);
	Servo_SetAngle_PB6(10,0.08,flas);
	Delay_ms(1000);
	Servo_SetAngle_PB7(145,0.08,flas);
	Delay_ms(1000);
	Servo_SetAngle_PB8(10,0.08,flas);
	Delay_ms(1000);
	Servo_SetAngle_PB9(10,0.1,flas);
}

